home *** CD-ROM | disk | FTP | other *** search
/ BCI NET 2 / BCI NET 2.iso / archives / programming / source / graphicgems4.lha / GemsIV / vec_mat / algebra3.c next >
Encoding:
C/C++ Source or Header  |  1995-02-06  |  22.8 KB  |  844 lines
  1. #include "algebra3.H"
  2. #include <ctype.h>
  3.  
  4. /****************************************************************
  5. *                                *
  6. *            vec2 Member functions            *
  7. *                                *
  8. ****************************************************************/
  9.  
  10. // CONSTRUCTORS
  11.  
  12. vec2::vec2() {}
  13.  
  14. vec2::vec2(const double x, const double y)
  15. { n[VX] = x; n[VY] = y; }
  16.  
  17. vec2::vec2(const double d)
  18. { n[VX] = n[VY] = d; }
  19.  
  20. vec2::vec2(const vec2& v)
  21. { n[VX] = v.n[VX]; n[VY] = v.n[VY]; }
  22.  
  23. vec2::vec2(const vec3& v) // it is up to caller to avoid divide-by-zero
  24. { n[VX] = v.n[VX]/v.n[VZ]; n[VY] = v.n[VY]/v.n[VZ]; };
  25.  
  26. vec2::vec2(const vec3& v, int dropAxis) {
  27.     switch (dropAxis) {
  28.     case VX: n[VX] = v.n[VY]; n[VY] = v.n[VZ]; break;
  29.     case VY: n[VX] = v.n[VX]; n[VY] = v.n[VZ]; break;
  30.     default: n[VX] = v.n[VX]; n[VY] = v.n[VY]; break;
  31.     }
  32. }
  33.  
  34.  
  35. // ASSIGNMENT OPERATORS
  36.  
  37. vec2& vec2::operator = (const vec2& v)
  38. { n[VX] = v.n[VX]; n[VY] = v.n[VY]; return *this; }
  39.  
  40. vec2& vec2::operator += ( const vec2& v )
  41. { n[VX] += v.n[VX]; n[VY] += v.n[VY]; return *this; }
  42.  
  43. vec2& vec2::operator -= ( const vec2& v )
  44. { n[VX] -= v.n[VX]; n[VY] -= v.n[VY]; return *this; }
  45.  
  46. vec2& vec2::operator *= ( const double d )
  47. { n[VX] *= d; n[VY] *= d; return *this; }
  48.  
  49. vec2& vec2::operator /= ( const double d )
  50. { double d_inv = 1./d; n[VX] *= d_inv; n[VY] *= d_inv; return *this; }
  51.  
  52. double& vec2::operator [] ( int i) {
  53.     if (i < VX || i > VY)
  54.     V_ERROR("vec2 [] operator: illegal access; index = " << i << '\n')
  55.     return n[i];
  56. }
  57.  
  58.  
  59. // SPECIAL FUNCTIONS
  60.  
  61. double vec2::length()
  62. { return sqrt(length2()); }
  63.  
  64. double vec2::length2()
  65. { return n[VX]*n[VX] + n[VY]*n[VY]; }
  66.  
  67. vec2& vec2::normalize() // it is up to caller to avoid divide-by-zero
  68. { *this /= length(); return *this; }
  69.  
  70. vec2& vec2::apply(V_FCT_PTR fct)
  71. { n[VX] = (*fct)(n[VX]); n[VY] = (*fct)(n[VY]); return *this; }
  72.  
  73.  
  74. // FRIENDS
  75.  
  76. vec2 operator - (const vec2& a)
  77. { return vec2(-a.n[VX],-a.n[VY]); }
  78.  
  79. vec2 operator + (const vec2& a, const vec2& b)
  80. { return vec2(a.n[VX]+ b.n[VX], a.n[VY] + b.n[VY]); }
  81.  
  82. vec2 operator - (const vec2& a, const vec2& b)
  83. { return vec2(a.n[VX]-b.n[VX], a.n[VY]-b.n[VY]); }
  84.  
  85. vec2 operator * (const vec2& a, const double d)
  86. { return vec2(d*a.n[VX], d*a.n[VY]); }
  87.  
  88. vec2 operator * (const double d, const vec2& a)
  89. { return a*d; }
  90.  
  91. vec2 operator * (const mat3& a, const vec2& v) {
  92.     vec3 av;
  93.  
  94.     av.n[VX] = a.v[0].n[VX]*v.n[VX] + a.v[0].n[VY]*v.n[VY] + a.v[0].n[VZ];
  95.     av.n[VY] = a.v[1].n[VX]*v.n[VX] + a.v[1].n[VY]*v.n[VY] + a.v[1].n[VZ];
  96.     av.n[VZ] = a.v[2].n[VX]*v.n[VX] + a.v[2].n[VY]*v.n[VY] + a.v[2].n[VZ];
  97.     return av;
  98. }
  99.  
  100. vec2 operator * (const vec2& v, mat3& a)
  101. { return a.transpose() * v; }
  102.  
  103. double operator * (const vec2& a, const vec2& b)
  104. { return (a.n[VX]*b.n[VX] + a.n[VY]*b.n[VY]); }
  105.  
  106. vec2 operator / (const vec2& a, const double d)
  107. { double d_inv = 1./d; return vec2(a.n[VX]*d_inv, a.n[VY]*d_inv); }
  108.  
  109. vec3 operator ^ (const vec2& a, const vec2& b)
  110. { return vec3(0.0, 0.0, a.n[VX] * b.n[VY] - b.n[VX] * a.n[VY]); }
  111.  
  112. int operator == (const vec2& a, const vec2& b)
  113. { return (a.n[VX] == b.n[VX]) && (a.n[VY] == b.n[VY]); }
  114.  
  115. int operator != (const vec2& a, const vec2& b)
  116. { return !(a == b); }
  117.  
  118. ostream& operator << (ostream& s, vec2& v)
  119. { return s << "| " << v.n[VX] << ' ' << v.n[VY] << " |"; }
  120.  
  121. istream& operator >> (istream& s, vec2& v) {
  122.     vec2    v_tmp;
  123.     char    c = ' ';
  124.  
  125.     while (isspace(c))
  126.     s >> c;
  127.     // The vectors can be formatted either as x y or | x y |
  128.     if (c == '|') {
  129.     s >> v_tmp[VX] >> v_tmp[VY];
  130.     while (s >> c && isspace(c)) ;
  131.     if (c != '|')
  132.         s.set(_bad);
  133.     }
  134.     else {
  135.     s.putback(c);
  136.     s >> v_tmp[VX] >> v_tmp[VY];
  137.     }
  138.     if (s)
  139.     v = v_tmp;
  140.     return s;
  141. }
  142.  
  143. void swap(vec2& a, vec2& b)
  144. { vec2 tmp(a); a = b; b = tmp; }
  145.  
  146. vec2 min(const vec2& a, const vec2& b)
  147. { return vec2(MIN(a.n[VX], b.n[VX]), MIN(a.n[VY], b.n[VY])); }
  148.  
  149. vec2 max(const vec2& a, const vec2& b)
  150. { return vec2(MAX(a.n[VX], b.n[VX]), MAX(a.n[VY], b.n[VY])); }
  151.  
  152. vec2 prod(const vec2& a, const vec2& b)
  153. { return vec2(a.n[VX] * b.n[VX], a.n[VY] * b.n[VY]); }
  154.  
  155. /****************************************************************
  156. *                                *
  157. *            vec3 Member functions            *
  158. *                                *
  159. ****************************************************************/
  160.  
  161. // CONSTRUCTORS
  162.  
  163. vec3::vec3() {}
  164.  
  165. vec3::vec3(const double x, const double y, const double z)
  166. { n[VX] = x; n[VY] = y; n[VZ] = z; }
  167.  
  168. vec3::vec3(const double d)
  169. { n[VX] = n[VY] = n[VZ] = d; }
  170.  
  171. vec3::vec3(const vec3& v)
  172. { n[VX] = v.n[VX]; n[VY] = v.n[VY]; n[VZ] = v.n[VZ]; }
  173.  
  174. vec3::vec3(const vec2& v)
  175. { n[VX] = v.n[VX]; n[VY] = v.n[VY]; n[VZ] = 1.0; }
  176.  
  177. vec3::vec3(const vec2& v, double d)
  178. { n[VX] = v.n[VX]; n[VY] = v.n[VY]; n[VZ] = d; }
  179.  
  180. vec3::vec3(const vec4& v) // it is up to caller to avoid divide-by-zero
  181. { n[VX] = v.n[VX] / v.n[VW]; n[VY] = v.n[VY] / v.n[VW];
  182.   n[VZ] = v.n[VZ] / v.n[VW]; }
  183.  
  184. vec3::vec3(const vec4& v, int dropAxis) {
  185.     switch (dropAxis) {
  186.     case VX: n[VX] = v.n[VY]; n[VY] = v.n[VZ]; n[VZ] = v.n[VW]; break;
  187.     case VY: n[VX] = v.n[VX]; n[VY] = v.n[VZ]; n[VZ] = v.n[VW]; break;
  188.     case VZ: n[VX] = v.n[VX]; n[VY] = v.n[VY]; n[VZ] = v.n[VW]; break;
  189.     default: n[VX] = v.n[VX]; n[VY] = v.n[VY]; n[VZ] = v.n[VZ]; break;
  190.     }
  191. }
  192.  
  193.  
  194. // ASSIGNMENT OPERATORS
  195.  
  196. vec3& vec3::operator = (const vec3& v)
  197. { n[VX] = v.n[VX]; n[VY] = v.n[VY]; n[VZ] = v.n[VZ]; return *this; }
  198.  
  199. vec3& vec3::operator += ( const vec3& v )
  200. { n[VX] += v.n[VX]; n[VY] += v.n[VY]; n[VZ] += v.n[VZ]; return *this; }
  201.  
  202. vec3& vec3::operator -= ( const vec3& v )
  203. { n[VX] -= v.n[VX]; n[VY] -= v.n[VY]; n[VZ] -= v.n[VZ]; return *this; }
  204.  
  205. vec3& vec3::operator *= ( const double d )
  206. { n[VX] *= d; n[VY] *= d; n[VZ] *= d; return *this; }
  207.  
  208. vec3& vec3::operator /= ( const double d )
  209. { double d_inv = 1./d; n[VX] *= d_inv; n[VY] *= d_inv; n[VZ] *= d_inv;
  210.   return *this; }
  211.  
  212. double& vec3::operator [] ( int i) {
  213.     if (i < VX || i > VZ)
  214.     V_ERROR("vec3 [] operator: illegal access; index = " << i << '\n')
  215.     return n[i];
  216. }
  217.  
  218.  
  219. // SPECIAL FUNCTIONS
  220.  
  221. double vec3::length()
  222. {  return sqrt(length2()); }
  223.  
  224. double vec3::length2()
  225. {  return n[VX]*n[VX] + n[VY]*n[VY] + n[VZ]*n[VZ]; }
  226.  
  227. vec3& vec3::normalize() // it is up to caller to avoid divide-by-zero
  228. { *this /= length(); return *this; }
  229.  
  230. vec3& vec3::apply(V_FCT_PTR fct)
  231. { n[VX] = (*fct)(n[VX]); n[VY] = (*fct)(n[VY]); n[VZ] = (*fct)(n[VZ]);
  232. return *this; }
  233.  
  234.  
  235. // FRIENDS
  236.  
  237. vec3 operator - (const vec3& a)
  238. {  return vec3(-a.n[VX],-a.n[VY],-a.n[VZ]); }
  239.  
  240. vec3 operator + (const vec3& a, const vec3& b)
  241. { return vec3(a.n[VX]+ b.n[VX], a.n[VY] + b.n[VY], a.n[VZ] + b.n[VZ]); }
  242.  
  243. vec3 operator - (const vec3& a, const vec3& b)
  244. { return vec3(a.n[VX]-b.n[VX], a.n[VY]-b.n[VY], a.n[VZ]-b.n[VZ]); }
  245.  
  246. vec3 operator * (const vec3& a, const double d)
  247. { return vec3(d*a.n[VX], d*a.n[VY], d*a.n[VZ]); }
  248.  
  249. vec3 operator * (const double d, const vec3& a)
  250. { return a*d; }
  251.  
  252. vec3 operator * (const mat4& a, const vec3& v)
  253. { return a * vec4(v); }
  254.  
  255. vec3 operator * (const vec3& v, mat4& a)
  256. { return a.transpose() * v; }
  257.  
  258. double operator * (const vec3& a, const vec3& b)
  259. { return (a.n[VX]*b.n[VX] + a.n[VY]*b.n[VY] + a.n[VZ]*b.n[VZ]); }
  260.  
  261. vec3 operator / (const vec3& a, const double d)
  262. { double d_inv = 1./d; return vec3(a.n[VX]*d_inv, a.n[VY]*d_inv,
  263.   a.n[VZ]*d_inv); }
  264.  
  265. vec3 operator ^ (const vec3& a, const vec3& b) {
  266.     return vec3(a.n[VY]*b.n[VZ] - a.n[VZ]*b.n[VY],
  267.         a.n[VZ]*b.n[VX] - a.n[VX]*b.n[VZ],
  268.         a.n[VX]*b.n[VY] - a.n[VY]*b.n[VX]);
  269. }
  270.  
  271. int operator == (const vec3& a, const vec3& b)
  272. { return (a.n[VX] == b.n[VX]) && (a.n[VY] == b.n[VY]) && (a.n[VZ] == b.n[VZ]);
  273. }
  274.  
  275. int operator != (const vec3& a, const vec3& b)
  276. { return !(a == b); }
  277.  
  278. ostream& operator << (ostream& s, vec3& v)
  279. { return s << "| " << v.n[VX] << ' ' << v.n[VY] << ' ' << v.n[VZ] << " |"; }
  280.  
  281. istream& operator >> (istream& s, vec3& v) {
  282.     vec3    v_tmp;
  283.     char    c = ' ';
  284.  
  285.     while (isspace(c))
  286.     s >> c;
  287.     // The vectors can be formatted either as x y z or | x y z |
  288.     if (c == '|') {
  289.     s >> v_tmp[VX] >> v_tmp[VY] >> v_tmp[VZ];
  290.     while (s >> c && isspace(c)) ;
  291.     if (c != '|')
  292.         s.set(_bad);
  293.     }
  294.     else {
  295.     s.putback(c);
  296.     s >> v_tmp[VX] >> v_tmp[VY] >> v_tmp[VZ];
  297.     }
  298.     if (s)
  299.     v = v_tmp;
  300.     return s;
  301. }
  302.  
  303. void swap(vec3& a, vec3& b)
  304. { vec3 tmp(a); a = b; b = tmp; }
  305.  
  306. vec3 min(const vec3& a, const vec3& b)
  307. { return vec3(MIN(a.n[VX], b.n[VX]), MIN(a.n[VY], b.n[VY]), MIN(a.n[VZ],
  308.   b.n[VZ])); }
  309.  
  310. vec3 max(const vec3& a, const vec3& b)
  311. { return vec3(MAX(a.n[VX], b.n[VX]), MAX(a.n[VY], b.n[VY]), MAX(a.n[VZ],
  312.   b.n[VZ])); }
  313.  
  314. vec3 prod(const vec3& a, const vec3& b)
  315. { return vec3(a.n[VX] * b.n[VX], a.n[VY] * b.n[VY], a.n[VZ] * b.n[VZ]); }
  316.  
  317.  
  318. /****************************************************************
  319. *                                *
  320. *            vec4 Member functions            *
  321. *                                *
  322. ****************************************************************/
  323.  
  324. // CONSTRUCTORS
  325.  
  326. vec4::vec4() {}
  327.  
  328. vec4::vec4(const double x, const double y, const double z, const double w)
  329. { n[VX] = x; n[VY] = y; n[VZ] = z; n[VW] = w; }
  330.  
  331. vec4::vec4(const double d)
  332. {  n[VX] = n[VY] = n[VZ] = n[VW] = d; }
  333.  
  334. vec4::vec4(const vec4& v)
  335. { n[VX] = v.n[VX]; n[VY] = v.n[VY]; n[VZ] = v.n[VZ]; n[VW] = v.n[VW]; }
  336.  
  337. vec4::vec4(const vec3& v)
  338. { n[VX] = v.n[VX]; n[VY] = v.n[VY]; n[VZ] = v.n[VZ]; n[VW] = 1.0; }
  339.  
  340. vec4::vec4(const vec3& v, const double d)
  341. { n[VX] = v.n[VX]; n[VY] = v.n[VY]; n[VZ] = v.n[VZ];  n[VW] = d; }
  342.  
  343.  
  344. // ASSIGNMENT OPERATORS
  345.  
  346. vec4& vec4::operator = (const vec4& v)
  347. { n[VX] = v.n[VX]; n[VY] = v.n[VY]; n[VZ] = v.n[VZ]; n[VW] = v.n[VW];
  348. return *this; }
  349.  
  350. vec4& vec4::operator += ( const vec4& v )
  351. { n[VX] += v.n[VX]; n[VY] += v.n[VY]; n[VZ] += v.n[VZ]; n[VW] += v.n[VW];
  352. return *this; }
  353.  
  354. vec4& vec4::operator -= ( const vec4& v )
  355. { n[VX] -= v.n[VX]; n[VY] -= v.n[VY]; n[VZ] -= v.n[VZ]; n[VW] -= v.n[VW];
  356. return *this; }
  357.  
  358. vec4& vec4::operator *= ( const double d )
  359. { n[VX] *= d; n[VY] *= d; n[VZ] *= d; n[VW] *= d; return *this; }
  360.  
  361. vec4& vec4::operator /= ( const double d )
  362. { double d_inv = 1./d; n[VX] *= d_inv; n[VY] *= d_inv; n[VZ] *= d_inv;
  363.   n[VW] *= d_inv; return *this; }
  364.  
  365. double& vec4::operator [] ( int i) {
  366.     if (i < VX || i > VW)
  367.     V_ERROR("vec4 [] operator: illegal access; index = " << i << '\n')
  368.     return n[i];
  369. }
  370.  
  371.  
  372. // SPECIAL FUNCTIONS
  373.  
  374. double vec4::length()
  375. { return sqrt(length2()); }
  376.  
  377. double vec4::length2()
  378. { return n[VX]*n[VX] + n[VY]*n[VY] + n[VZ]*n[VZ] + n[VW]*n[VW]; }
  379.  
  380. vec4& vec4::normalize() // it is up to caller to avoid divide-by-zero
  381. { *this /= length(); return *this; }
  382.  
  383. vec4& vec4::apply(V_FCT_PTR fct)
  384. { n[VX] = (*fct)(n[VX]); n[VY] = (*fct)(n[VY]); n[VZ] = (*fct)(n[VZ]);
  385. n[VW] = (*fct)(n[VW]); return *this; }
  386.  
  387.  
  388. // FRIENDS
  389.  
  390. vec4 operator - (const vec4& a)
  391. { return vec4(-a.n[VX],-a.n[VY],-a.n[VZ],-a.n[VW]); }
  392.  
  393. vec4 operator + (const vec4& a, const vec4& b)
  394. { return vec4(a.n[VX] + b.n[VX], a.n[VY] + b.n[VY], a.n[VZ] + b.n[VZ],
  395.   a.n[VW] + b.n[VW]); }
  396.  
  397. vec4 operator - (const vec4& a, const vec4& b)
  398. {  return vec4(a.n[VX] - b.n[VX], a.n[VY] - b.n[VY], a.n[VZ] - b.n[VZ],
  399.    a.n[VW] - b.n[VW]); }
  400.  
  401. vec4 operator * (const vec4& a, const double d)
  402. { return vec4(d*a.n[VX], d*a.n[VY], d*a.n[VZ], d*a.n[VW] ); }
  403.  
  404. vec4 operator * (const double d, const vec4& a)
  405. { return a*d; }
  406.  
  407. vec4 operator * (const mat4& a, const vec4& v) {
  408.     #define ROWCOL(i) a.v[i].n[0]*v.n[VX] + a.v[i].n[1]*v.n[VY] \
  409.     + a.v[i].n[2]*v.n[VZ] + a.v[i].n[3]*v.n[VW]
  410.     return vec4(ROWCOL(0), ROWCOL(1), ROWCOL(2), ROWCOL(3));
  411.     #undef ROWCOL(i)
  412. }
  413.  
  414. vec4 operator * (const vec4& v, mat4& a)
  415. { return a.transpose() * v; }
  416.  
  417. double operator * (const vec4& a, const vec4& b)
  418. { return (a.n[VX]*b.n[VX] + a.n[VY]*b.n[VY] + a.n[VZ]*b.n[VZ] +
  419.   a.n[VW]*b.n[VW]); }
  420.  
  421. vec4 operator / (const vec4& a, const double d)
  422. { double d_inv = 1./d; return vec4(a.n[VX]*d_inv, a.n[VY]*d_inv, a.n[VZ]*d_inv,
  423.   a.n[VW]*d_inv); }
  424.  
  425. int operator == (const vec4& a, const vec4& b)
  426. { return (a.n[VX] == b.n[VX]) && (a.n[VY] == b.n[VY]) && (a.n[VZ] == b.n[VZ])
  427.   && (a.n[VW] == b.n[VW]); }
  428.  
  429. int operator != (const vec4& a, const vec4& b)
  430. { return !(a == b); }
  431.  
  432. ostream& operator << (ostream& s, vec4& v)
  433. { return s << "| " << v.n[VX] << ' ' << v.n[VY] << ' ' << v.n[VZ] << ' '
  434.   << v.n[VW] << " |"; }
  435.  
  436. istream& operator >> (istream& s, vec4& v) {
  437.     vec4    v_tmp;
  438.     char    c = ' ';
  439.  
  440.     while (isspace(c))
  441.     s >> c;
  442.     // The vectors can be formatted either as x y z w or | x y z w |
  443.     if (c == '|') {
  444.     s >> v_tmp[VX] >> v_tmp[VY] >> v_tmp[VZ] >> v_tmp[VW];
  445.     while (s >> c && isspace(c)) ;
  446.     if (c != '|')
  447.         s.set(_bad);
  448.     }
  449.     else {
  450.     s.putback(c);
  451.     s >> v_tmp[VX] >> v_tmp[VY] >> v_tmp[VZ] >> v_tmp[VW];
  452.     }
  453.     if (s)
  454.     v = v_tmp;
  455.     return s;
  456. }
  457.  
  458. void swap(vec4& a, vec4& b)
  459. { vec4 tmp(a); a = b; b = tmp; }
  460.  
  461. vec4 min(const vec4& a, const vec4& b)
  462. { return vec4(MIN(a.n[VX], b.n[VX]), MIN(a.n[VY], b.n[VY]), MIN(a.n[VZ],
  463.   b.n[VZ]), MIN(a.n[VW], b.n[VW])); }
  464.  
  465. vec4 max(const vec4& a, const vec4& b)
  466. { return vec4(MAX(a.n[VX], b.n[VX]), MAX(a.n[VY], b.n[VY]), MAX(a.n[VZ],
  467.   b.n[VZ]), MAX(a.n[VW], b.n[VW])); }
  468.  
  469. vec4 prod(const vec4& a, const vec4& b)
  470. { return vec4(a.n[VX] * b.n[VX], a.n[VY] * b.n[VY], a.n[VZ] * b.n[VZ],
  471.   a.n[VW] * b.n[VW]); }
  472.  
  473.  
  474. /****************************************************************
  475. *                                *
  476. *            mat3 member functions            *
  477. *                                *
  478. ****************************************************************/
  479.  
  480. // CONSTRUCTORS
  481.  
  482. mat3::mat3() {}
  483.  
  484. mat3::mat3(const vec3& v0, const vec3& v1, const vec3& v2)
  485. { v[0] = v0; v[1] = v1; v[2] = v2; }
  486.  
  487. mat3::mat3(const double d)
  488. { v[0] = v[1] = v[2] = vec3(d); }
  489.  
  490. mat3::mat3(const mat3& m)
  491. { v[0] = m.v[0]; v[1] = m.v[1]; v[2] = m.v[2]; }
  492.  
  493.  
  494. // ASSIGNMENT OPERATORS
  495.  
  496. mat3& mat3::operator = ( const mat3& m )
  497. { v[0] = m.v[0]; v[1] = m.v[1]; v[2] = m.v[2]; return *this; }
  498.  
  499. mat3& mat3::operator += ( const mat3& m )
  500. { v[0] += m.v[0]; v[1] += m.v[1]; v[2] += m.v[2]; return *this; }
  501.  
  502. mat3& mat3::operator -= ( const mat3& m )
  503. { v[0] -= m.v[0]; v[1] -= m.v[1]; v[2] -= m.v[2]; return *this; }
  504.  
  505. mat3& mat3::operator *= ( const double d )
  506. { v[0] *= d; v[1] *= d; v[2] *= d; return *this; }
  507.  
  508. mat3& mat3::operator /= ( const double d )
  509. { v[0] /= d; v[1] /= d; v[2] /= d; return *this; }
  510.  
  511. vec3& mat3::operator [] ( int i) {
  512.     if (i < VX || i > VZ)
  513.     V_ERROR("mat3 [] operator: illegal access; index = " << i << '\n')
  514.     return v[i];
  515. }
  516.  
  517.  
  518. // SPECIAL FUNCTIONS
  519.  
  520. mat3 mat3::transpose() {
  521.     return mat3(vec3(v[0][0], v[1][0], v[2][0]),
  522.         vec3(v[0][1], v[1][1], v[2][1]),
  523.         vec3(v[0][2], v[1][2], v[2][2]));
  524. }
  525.  
  526. mat3 mat3::inverse()        // Gauss-Jordan elimination with partial pivoting
  527.     {
  528.     mat3 a(*this),        // As a evolves from original mat into identity
  529.      b(identity2D());   // b evolves from identity into inverse(a)
  530.     int     i, j, i1;
  531.  
  532.     // Loop over cols of a from left to right, eliminating above and below diag
  533.     for (j=0; j<3; j++) {   // Find largest pivot in column j among rows j..2
  534.     i1 = j;            // Row with largest pivot candidate
  535.     for (i=j+1; i<3; i++)
  536.     if (fabs(a.v[i].n[j]) > fabs(a.v[i1].n[j]))
  537.         i1 = i;
  538.  
  539.     // Swap rows i1 and j in a and b to put pivot on diagonal
  540.     swap(a.v[i1], a.v[j]);
  541.     swap(b.v[i1], b.v[j]);
  542.  
  543.     // Scale row j to have a unit diagonal
  544.     if (a.v[j].n[j]==0.)
  545.     V_ERROR("mat3::inverse: singular matrix; can't invert\n")
  546.     b.v[j] /= a.v[j].n[j];
  547.     a.v[j] /= a.v[j].n[j];
  548.  
  549.     // Eliminate off-diagonal elems in col j of a, doing identical ops to b
  550.     for (i=0; i<3; i++)
  551.     if (i!=j) {
  552.     b.v[i] -= a.v[i].n[j]*b.v[j];
  553.     a.v[i] -= a.v[i].n[j]*a.v[j];
  554.     }
  555.     }
  556.     return b;
  557. }
  558.  
  559. mat3& mat3::apply(V_FCT_PTR fct) {
  560.     v[VX].apply(fct);
  561.     v[VY].apply(fct);
  562.     v[VZ].apply(fct);
  563.     return *this;
  564. }
  565.  
  566.  
  567. // FRIENDS
  568.  
  569. mat3 operator - (const mat3& a)
  570. { return mat3(-a.v[0], -a.v[1], -a.v[2]); }
  571.  
  572. mat3 operator + (const mat3& a, const mat3& b)
  573. { return mat3(a.v[0] + b.v[0], a.v[1] + b.v[1], a.v[2] + b.v[2]); }
  574.  
  575. mat3 operator - (const mat3& a, const mat3& b)
  576. { return mat3(a.v[0] - b.v[0], a.v[1] - b.v[1], a.v[2] - b.v[2]); }
  577.  
  578. mat3 operator * (mat3& a, mat3& b) {
  579.     #define ROWCOL(i, j) \
  580.     a.v[i].n[0]*b.v[0][j] + a.v[i].n[1]*b.v[1][j] + a.v[i].n[2]*b.v[2][j]
  581.     return mat3(vec3(ROWCOL(0,0), ROWCOL(0,1), ROWCOL(0,2)),
  582.         vec3(ROWCOL(1,0), ROWCOL(1,1), ROWCOL(1,2)),
  583.         vec3(ROWCOL(2,0), ROWCOL(2,1), ROWCOL(2,2)));
  584.     #undef ROWCOL(i, j)
  585. }
  586.  
  587. mat3 operator * (const mat3& a, const double d)
  588. { return mat3(a.v[0] * d, a.v[1] * d, a.v[2] * d); }
  589.  
  590. mat3 operator * (const double d, const mat3& a)
  591. { return a*d; }
  592.  
  593. mat3 operator / (const mat3& a, const double d)
  594. { return mat3(a.v[0] / d, a.v[1] / d, a.v[2] / d); }
  595.  
  596. int operator == (const mat3& a, const mat3& b)
  597. { return (a.v[0] == b.v[0]) && (a.v[1] == b.v[1]) && (a.v[2] == b.v[2]); }
  598.  
  599. int operator != (const mat3& a, const mat3& b)
  600. { return !(a == b); }
  601.  
  602. ostream& operator << (ostream& s, mat3& m)
  603. { return s << m.v[VX] << '\n' << m.v[VY] << '\n' << m.v[VZ]; }
  604.  
  605. istream& operator >> (istream& s, mat3& m) {
  606.     mat3    m_tmp;
  607.  
  608.     s >> m_tmp[VX] >> m_tmp[VY] >> m_tmp[VZ];
  609.     if (s)
  610.     m = m_tmp;
  611.     return s;
  612. }
  613.  
  614. void swap(mat3& a, mat3& b)
  615. { mat3 tmp(a); a = b; b = tmp; }
  616.  
  617.  
  618. /****************************************************************
  619. *                                *
  620. *            mat4 member functions            *
  621. *                                *
  622. ****************************************************************/
  623.  
  624. // CONSTRUCTORS
  625.  
  626. mat4::mat4() {}
  627.  
  628. mat4::mat4(const vec4& v0, const vec4& v1, const vec4& v2, const vec4& v3)
  629. { v[0] = v0; v[1] = v1; v[2] = v2; v[3] = v3; }
  630.  
  631. mat4::mat4(const double d)
  632. { v[0] = v[1] = v[2] = v[3] = vec4(d); }
  633.  
  634. mat4::mat4(const mat4& m)
  635. { v[0] = m.v[0]; v[1] = m.v[1]; v[2] = m.v[2]; v[3] = m.v[3]; }
  636.  
  637.  
  638. // ASSIGNMENT OPERATORS
  639.  
  640. mat4& mat4::operator = ( const mat4& m )
  641. { v[0] = m.v[0]; v[1] = m.v[1]; v[2] = m.v[2]; v[3] = m.v[3];
  642. return *this; }
  643.  
  644. mat4& mat4::operator += ( const mat4& m )
  645. { v[0] += m.v[0]; v[1] += m.v[1]; v[2] += m.v[2]; v[3] += m.v[3];
  646. return *this; }
  647.  
  648. mat4& mat4::operator -= ( const mat4& m )
  649. { v[0] -= m.v[0]; v[1] -= m.v[1]; v[2] -= m.v[2]; v[3] -= m.v[3];
  650. return *this; }
  651.  
  652. mat4& mat4::operator *= ( const double d )
  653. { v[0] *= d; v[1] *= d; v[2] *= d; v[3] *= d; return *this; }
  654.  
  655. mat4& mat4::operator /= ( const double d )
  656. { v[0] /= d; v[1] /= d; v[2] /= d; v[3] /= d; return *this; }
  657.  
  658. vec4& mat4::operator [] ( int i) {
  659.     if (i < VX || i > VW)
  660.     V_ERROR("mat4 [] operator: illegal access; index = " << i << '\n')
  661.     return v[i];
  662. }
  663.  
  664. // SPECIAL FUNCTIONS;
  665.  
  666. mat4 mat4::transpose() {
  667.     return mat4(vec4(v[0][0], v[1][0], v[2][0], v[3][0]),
  668.         vec4(v[0][1], v[1][1], v[2][1], v[3][1]),
  669.         vec4(v[0][2], v[1][2], v[2][2], v[3][2]),
  670.         vec4(v[0][3], v[1][3], v[2][3], v[3][3]));
  671. }
  672.  
  673. mat4 mat4::inverse()        // Gauss-Jordan elimination with partial pivoting
  674. {
  675.     mat4 a(*this),        // As a evolves from original mat into identity
  676.      b(identity3D());   // b evolves from identity into inverse(a)
  677.     int i, j, i1;
  678.  
  679.     // Loop over cols of a from left to right, eliminating above and below diag
  680.     for (j=0; j<4; j++) {   // Find largest pivot in column j among rows j..3
  681.     i1 = j;            // Row with largest pivot candidate
  682.     for (i=j+1; i<4; i++)
  683.     if (fabs(a.v[i].n[j]) > fabs(a.v[i1].n[j]))
  684.     i1 = i;
  685.  
  686.     // Swap rows i1 and j in a and b to put pivot on diagonal
  687.     swap(a.v[i1], a.v[j]);
  688.     swap(b.v[i1], b.v[j]);
  689.  
  690.     // Scale row j to have a unit diagonal
  691.     if (a.v[j].n[j]==0.)
  692.     V_ERROR("mat4::inverse: singular matrix; can't invert\n");
  693.     b.v[j] /= a.v[j].n[j];
  694.     a.v[j] /= a.v[j].n[j];
  695.  
  696.     // Eliminate off-diagonal elems in col j of a, doing identical ops to b
  697.     for (i=0; i<4; i++)
  698.     if (i!=j) {
  699.     b.v[i] -= a.v[i].n[j]*b.v[j];
  700.     a.v[i] -= a.v[i].n[j]*a.v[j];
  701.     }
  702.     }
  703.     return b;
  704. }
  705.  
  706. mat4& mat4::apply(V_FCT_PTR fct)
  707. { v[VX].apply(fct); v[VY].apply(fct); v[VZ].apply(fct); v[VW].apply(fct);
  708. return *this; }
  709.  
  710.  
  711. // FRIENDS
  712.  
  713. mat4 operator - (const mat4& a)
  714. { return mat4(-a.v[0], -a.v[1], -a.v[2], -a.v[3]); }
  715.  
  716. mat4 operator + (const mat4& a, const mat4& b)
  717. { return mat4(a.v[0] + b.v[0], a.v[1] + b.v[1], a.v[2] + b.v[2],
  718.   a.v[3] + b.v[3]);
  719. }
  720.  
  721. mat4 operator - (const mat4& a, const mat4& b)
  722. { return mat4(a.v[0] - b.v[0], a.v[1] - b.v[1], a.v[2] - b.v[2], a.v[3] - b.v[3]); }
  723.  
  724. mat4 operator * (mat4& a, mat4& b) {
  725.     #define ROWCOL(i, j) a.v[i].n[0]*b.v[0][j] + a.v[i].n[1]*b.v[1][j] + \
  726.     a.v[i].n[2]*b.v[2][j] + a.v[i].n[3]*b.v[3][j]
  727.     return mat4(
  728.     vec4(ROWCOL(0,0), ROWCOL(0,1), ROWCOL(0,2), ROWCOL(0,3)),
  729.     vec4(ROWCOL(1,0), ROWCOL(1,1), ROWCOL(1,2), ROWCOL(1,3)),
  730.     vec4(ROWCOL(2,0), ROWCOL(2,1), ROWCOL(2,2), ROWCOL(2,3)),
  731.     vec4(ROWCOL(3,0), ROWCOL(3,1), ROWCOL(3,2), ROWCOL(3,3))
  732.     );
  733. }
  734.  
  735. mat4 operator * (const mat4& a, const double d)
  736. { return mat4(a.v[0] * d, a.v[1] * d, a.v[2] * d, a.v[3] * d); }
  737.  
  738. mat4 operator * (const double d, const mat4& a)
  739. { return a*d; }
  740.  
  741. mat4 operator / (const mat4& a, const double d)
  742. { return mat4(a.v[0] / d, a.v[1] / d, a.v[2] / d, a.v[3] / d); }
  743.  
  744. int operator == (const mat4& a, const mat4& b)
  745. { return ((a.v[0] == b.v[0]) && (a.v[1] == b.v[1]) && (a.v[2] == b.v[2]) &&
  746.   (a.v[3] == b.v[3])); }
  747.  
  748. int operator != (const mat4& a, const mat4& b)
  749. { return !(a == b); }
  750.  
  751. ostream& operator << (ostream& s, mat4& m)
  752. { return s << m.v[VX] << '\n' << m.v[VY] << '\n' << m.v[VZ] << '\n' << m.v[VW]; }
  753.  
  754. istream& operator >> (istream& s, mat4& m)
  755. {
  756.     mat4    m_tmp;
  757.  
  758.     s >> m_tmp[VX] >> m_tmp[VY] >> m_tmp[VZ] >> m_tmp[VW];
  759.     if (s)
  760.     m = m_tmp;
  761.     return s;
  762. }
  763.  
  764. void swap(mat4& a, mat4& b)
  765. { mat4 tmp(a); a = b; b = tmp; }
  766.  
  767.  
  768. /****************************************************************
  769. *                                *
  770. *           2D functions and 3D functions            *
  771. *                                *
  772. ****************************************************************/
  773.  
  774. mat3 identity2D()
  775. {   return mat3(vec3(1.0, 0.0, 0.0),
  776.         vec3(0.0, 1.0, 0.0),
  777.         vec3(0.0, 0.0, 1.0)); }
  778.  
  779. mat3 translation2D(vec2& v)
  780. {   return mat3(vec3(1.0, 0.0, v[VX]),
  781.         vec3(0.0, 1.0, v[VY]),
  782.         vec3(0.0, 0.0, 1.0)); }
  783.  
  784. mat3 rotation2D(vec2& Center, const double angleDeg) {
  785.     double  angleRad = angleDeg * M_PI / 180.0,
  786.         c = cos(angleRad),
  787.         s = sin(angleRad);
  788.  
  789.     return mat3(vec3(c, -s, Center[VX] * (1.0-c) + Center[VY] * s),
  790.         vec3(s, c, Center[VY] * (1.0-c) - Center[VX] * s),
  791.         vec3(0.0, 0.0, 1.0));
  792. }
  793.  
  794. mat3 scaling2D(vec2& scaleVector)
  795. {   return mat3(vec3(scaleVector[VX], 0.0, 0.0),
  796.         vec3(0.0, scaleVector[VY], 0.0),
  797.         vec3(0.0, 0.0, 1.0)); }
  798.  
  799. mat4 identity3D()
  800. {   return mat4(vec4(1.0, 0.0, 0.0, 0.0),
  801.         vec4(0.0, 1.0, 0.0, 0.0),
  802.         vec4(0.0, 0.0, 1.0, 0.0),
  803.         vec4(0.0, 0.0, 0.0, 1.0)); }
  804.  
  805. mat4 translation3D(vec3& v)
  806. {   return mat4(vec4(1.0, 0.0, 0.0, v[VX]),
  807.         vec4(0.0, 1.0, 0.0, v[VY]),
  808.         vec4(0.0, 0.0, 1.0, v[VZ]),
  809.         vec4(0.0, 0.0, 0.0, 1.0)); }
  810.  
  811. mat4 rotation3D(vec3& Axis, const double angleDeg) {
  812.     double  angleRad = angleDeg * M_PI / 180.0,
  813.         c = cos(angleRad),
  814.         s = sin(angleRad),
  815.         t = 1.0 - c;
  816.  
  817.     Axis.normalize();
  818.     return mat4(vec4(t * Axis[VX] * Axis[VX] + c,
  819.              t * Axis[VX] * Axis[VY] - s * Axis[VZ],
  820.              t * Axis[VX] * Axis[VZ] + s * Axis[VY],
  821.              0.0),
  822.         vec4(t * Axis[VX] * Axis[VY] + s * Axis[VZ],
  823.              t * Axis[VY] * Axis[VY] + c,
  824.              t * Axis[VY] * Axis[VZ] - s * Axis[VX],
  825.              0.0),
  826.         vec4(t * Axis[VX] * Axis[VZ] - s * Axis[VY],
  827.              t * Axis[VY] * Axis[VZ] + s * Axis[VX],
  828.              t * Axis[VZ] * Axis[VZ] + c,
  829.              0.0),
  830.         vec4(0.0, 0.0, 0.0, 1.0));
  831. }
  832.  
  833. mat4 scaling3D(vec3& scaleVector)
  834. {   return mat4(vec4(scaleVector[VX], 0.0, 0.0, 0.0),
  835.         vec4(0.0, scaleVector[VY], 0.0, 0.0),
  836.         vec4(0.0, 0.0, scaleVector[VZ], 0.0),
  837.         vec4(0.0, 0.0, 0.0, 1.0)); }
  838.  
  839. mat4 perspective3D(const double d)
  840. {   return mat4(vec4(1.0, 0.0, 0.0, 0.0),
  841.         vec4(0.0, 1.0, 0.0, 0.0),
  842.         vec4(0.0, 0.0, 1.0, 0.0),
  843.         vec4(0.0, 0.0, 1.0/d, 0.0)); }
  844.